/* *****************************************************************
    MESQUITE -- The Mesh Quality Improvement Toolkit

    Copyright 2004 Sandia Corporation and Argonne National
    Laboratory.  Under the terms of Contract DE-AC04-94AL85000
    with Sandia Corporation, the U.S. Government retains certain
    rights in this software.

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    (lgpl.txt) along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

    diachin2@llnl.gov, djmelan@sandia.gov, mbrewer@sandia.gov,
    pknupp@sandia.gov, tleurent@mcs.anl.gov, tmunson@mcs.anl.gov
    kraftche@cae.wisc.edu

  ***************************************************************** */
/*!
  \file   QualityMetric.cpp
  \brief

  \author Michael Brewer
  \author Thomas Leurent
  \date   2002-05-14
  \author Jason Kraftcheck
  \date   2006-04-20
*/

#include "QualityMetric.hpp"
#include "MsqVertex.hpp"
#include "PatchData.hpp"

namespace MBMesquite
{

void QualityMetric::initialize_queue( MeshDomainAssoc*, const Settings*, MsqError& ) {}

void QualityMetric::get_single_pass( PatchData& pd, std::vector< size_t >& handles, bool free_vertices_only,
                                     MsqError& err )
{
    get_evaluations( pd, handles, free_vertices_only, err );
}

static inline double get_delta_C( const PatchData& pd, const std::vector< size_t >& indices, MsqError& err )
{
    if( indices.empty() )
    {
        MSQ_SETERR( err )( MsqError::INVALID_ARG );
        return 0.0;
    }

    std::vector< size_t >::const_iterator beg, iter, iter2, end;

    std::vector< size_t > tmp_vect;
    if( indices.size() == 1u )
    {
        pd.get_adjacent_vertex_indices( indices.front(), tmp_vect, err );
        MSQ_ERRZERO( err );
        assert( !tmp_vect.empty() );
        tmp_vect.push_back( indices.front() );
        beg = tmp_vect.begin();
        end = tmp_vect.end();
    }
    else
    {
        beg = indices.begin();
        end = indices.end();
    }

    double min_dist_sqr = HUGE_VAL, sum_dist_sqr = 0.0;
    for( iter = beg; iter != end; ++iter )
    {
        for( iter2 = iter + 1; iter2 != end; ++iter2 )
        {
            Vector3D diff = pd.vertex_by_index( *iter );
            diff -= pd.vertex_by_index( *iter2 );
            double dist_sqr = diff % diff;
            if( dist_sqr < min_dist_sqr ) min_dist_sqr = dist_sqr;
            sum_dist_sqr += dist_sqr;
        }
    }

    return 3e-6 * sqrt( min_dist_sqr ) + 5e-7 * sqrt( sum_dist_sqr / ( end - beg ) );
}

bool QualityMetric::evaluate_with_gradient( PatchData& pd, size_t handle, double& value, std::vector< size_t >& indices,
                                            std::vector< Vector3D >& gradient, MsqError& err )
{
    indices.clear();
    bool valid = evaluate_with_indices( pd, handle, value, indices, err );
    if( MSQ_CHKERR( err ) || !valid ) return false;
    if( indices.empty() ) return true;

    // get initial pertubation amount
    double delta_C = finiteDiffEps;
    if( !haveFiniteDiffEps )
    {
        delta_C = get_delta_C( pd, indices, err );
        MSQ_ERRZERO( err );
        if( keepFiniteDiffEps )
        {
            finiteDiffEps     = delta_C;
            haveFiniteDiffEps = true;
        }
    }
    const double delta_inv_C  = 1.0 / delta_C;
    const int reduction_limit = 15;

    gradient.resize( indices.size() );
    for( size_t v = 0; v < indices.size(); ++v )
    {
        const Vector3D pos = pd.vertex_by_index( indices[v] );

        /* gradient in the x, y, z direction */
        for( int j = 0; j < 3; ++j )
        {
            double delta     = delta_C;
            double delta_inv = delta_inv_C;
            double metric_value;
            Vector3D delta_v( 0, 0, 0 );

            // perturb the node and calculate gradient.  The while loop is a
            // safety net to make sure the epsilon perturbation does not take
            // the element out of the feasible region.
            int counter = 0;
            for( ;; )
            {
                // perturb the coordinates of the free vertex in the j direction
                // by delta
                delta_v[j] = delta;
                pd.set_vertex_coordinates( pos + delta_v, indices[v], err );
                MSQ_ERRZERO( err );

                // compute the function at the perturbed point location
                valid = evaluate( pd, handle, metric_value, err );
                if( !MSQ_CHKERR( err ) && valid ) break;

                if( ++counter >= reduction_limit )
                {
                    MSQ_SETERR( err )
                    ( "Perturbing vertex by delta caused an inverted element.", MsqError::INTERNAL_ERROR );
                    return false;
                }

                delta *= 0.1;
                delta_inv *= 10.;
            }
            // put the coordinates back where they belong
            pd.set_vertex_coordinates( pos, indices[v], err );
            // compute the numerical gradient
            gradient[v][j] = ( metric_value - value ) * delta_inv;
        }  // for(j)
    }      // for(indices)
    return true;
}

bool QualityMetric::evaluate_with_Hessian( PatchData& pd, size_t handle, double& value, std::vector< size_t >& indices,
                                           std::vector< Vector3D >& gradient, std::vector< Matrix3D >& Hessian,
                                           MsqError& err )
{
    indices.clear();
    gradient.clear();
    keepFiniteDiffEps = true;
    bool valid        = evaluate_with_gradient( pd, handle, value, indices, gradient, err );
    keepFiniteDiffEps = false;
    if( MSQ_CHKERR( err ) || !valid )
    {
        haveFiniteDiffEps = false;
        return false;
    }
    if( indices.empty() )
    {
        haveFiniteDiffEps = false;
        return true;
    }

    // get initial pertubation amount
    double delta_C;
    if( haveFiniteDiffEps ) { delta_C = finiteDiffEps; }
    else
    {
        delta_C = get_delta_C( pd, indices, err );
        MSQ_ERRZERO( err );
    }
    assert( delta_C < 1e30 );
    const double delta_inv_C  = 1.0 / delta_C;
    const int reduction_limit = 15;

    std::vector< Vector3D > temp_gradient( indices.size() );
    const int num_hess = indices.size() * ( indices.size() + 1 ) / 2;
    Hessian.resize( num_hess );

    for( unsigned v = 0; v < indices.size(); ++v )
    {
        const Vector3D pos = pd.vertex_by_index( indices[v] );
        for( int j = 0; j < 3; ++j )
        {  // x, y, and z
            double delta     = delta_C;
            double delta_inv = delta_inv_C;
            double metric_value;
            Vector3D delta_v( 0, 0, 0 );

            // find finite difference for gradient
            int counter = 0;
            for( ;; )
            {
                delta_v[j] = delta;
                pd.set_vertex_coordinates( pos + delta_v, indices[v], err );
                MSQ_ERRZERO( err );
                valid = evaluate_with_gradient( pd, handle, metric_value, indices, temp_gradient, err );
                if( !MSQ_CHKERR( err ) && valid ) break;

                if( ++counter >= reduction_limit )
                {
                    MSQ_SETERR( err )
                    ( "Algorithm did not successfully compute element's "
                      "Hessian.\n",
                      MsqError::INTERNAL_ERROR );
                    haveFiniteDiffEps = false;
                    return false;
                }

                delta *= 0.1;
                delta_inv *= 10.0;
            }
            pd.set_vertex_coordinates( pos, indices[v], err );
            MSQ_ERRZERO( err );

            // compute the numerical Hessian
            for( unsigned w = 0; w <= v; ++w )
            {
                // finite difference to get some entries of the Hessian
                Vector3D fd( temp_gradient[w] );
                fd -= gradient[w];
                fd *= delta_inv;
                // For the block at position w,v in a matrix, we need the corresponding index
                // (mat_index) in a 1D array containing only upper triangular blocks.
                unsigned sum_w           = w * ( w + 1 ) / 2;  // 1+2+3+...+w
                unsigned mat_index       = w * indices.size() + v - sum_w;
                Hessian[mat_index][0][j] = fd[0];
                Hessian[mat_index][1][j] = fd[1];
                Hessian[mat_index][2][j] = fd[2];
            }
        }  // for(j)
    }      // for(indices)
    haveFiniteDiffEps = false;
    return true;
}

bool QualityMetric::evaluate_with_Hessian_diagonal( PatchData& pd, size_t handle, double& value,
                                                    std::vector< size_t >& indices, std::vector< Vector3D >& gradient,
                                                    std::vector< SymMatrix3D >& Hessian_diagonal, MsqError& err )
{
    bool rval = evaluate_with_Hessian( pd, handle, value, indices, gradient, tmpHess, err );
    if( MSQ_CHKERR( err ) || !rval ) return rval;
    size_t s = indices.size();
    Hessian_diagonal.resize( s );
    std::vector< Matrix3D >::const_iterator h = tmpHess.begin();
    for( size_t i = 0; i < indices.size(); ++i )
    {
        Hessian_diagonal[i] = h->upper();
        h += s--;
    }
    return rval;
}

uint32_t QualityMetric::fixed_vertex_bitmap( PatchData& pd, const MsqMeshEntity* elem, std::vector< size_t >& indices )
{
    indices.clear();
    uint32_t result        = ~(uint32_t)0;
    unsigned num_vtx       = elem->vertex_count();
    const size_t* vertices = elem->get_vertex_index_array();
    indices.clear();
    for( unsigned i = 0; i < num_vtx; ++i )
    {
        if( vertices[i] < pd.num_free_vertices() )
        {
            indices.push_back( vertices[i] );
            result &= ~( uint32_t )( 1 << i );
        }
    }
    return result;
}

void QualityMetric::remove_fixed_gradients( EntityTopology elem_type, uint32_t fixed, std::vector< Vector3D >& grads )
{
    const unsigned num_vertex = TopologyInfo::corners( elem_type );
    unsigned r, w;
    for( r = 0; r < num_vertex && !( fixed & ( 1 << r ) ); ++r )
        ;
    for( w = r++; r < num_vertex; ++r )
    {
        if( !( fixed & ( 1 << r ) ) )
        {
            grads[w] = grads[r];
            ++w;
        }
    }
    grads.resize( w );
}

void QualityMetric::remove_fixed_diagonals( EntityTopology type, uint32_t fixed, std::vector< Vector3D >& grads,
                                            std::vector< SymMatrix3D >& diags )
{
    const unsigned num_vertex = TopologyInfo::corners( type );
    unsigned r, w;
    for( r = 0; r < num_vertex && !( fixed & ( 1 << r ) ); ++r )
        ;
    for( w = r++; r < num_vertex; ++r )
    {
        if( !( fixed & ( 1 << r ) ) )
        {
            grads[w] = grads[r];
            diags[w] = diags[r];
            ++w;
        }
    }
    grads.resize( w );
    diags.resize( w );
}

void QualityMetric::remove_fixed_hessians( EntityTopology elem_type, uint32_t fixed, std::vector< Matrix3D >& hessians )
{
    const unsigned num_vertex = TopologyInfo::corners( elem_type );
    unsigned r, c, i = 0, w = 0;
    for( r = 0; r < num_vertex; ++r )
    {
        if( fixed & ( 1 << r ) )
        {
            i += num_vertex - r;
            continue;
        }
        for( c = r; c < num_vertex; ++c )
        {
            if( !( fixed & ( 1 << c ) ) )
            {
                hessians[w] = hessians[i];
                ++w;
            }
            ++i;
        }
    }
    hessians.resize( w );
}

double QualityMetric::weighted_average_metrics( const double coef[], const double metric_values[],
                                                const int& num_values, MsqError& /*err*/ )
{
    // MSQ_MAX needs to be made global?
    // double MSQ_MAX=1e10;
    double total_value = 0.0;
    int i              = 0;
    // if no values, return zero
    if( num_values <= 0 ) { return 0.0; }

    for( i = 0; i < num_values; ++i )
    {
        total_value += coef[i] * metric_values[i];
    }
    total_value /= (double)num_values;

    return total_value;
}

}  // namespace MBMesquite
